/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received _a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#pragma once

#include "Chain.h"
#include "MotionPlanning.h"

namespace RobotPlant { namespace Mechanism
{
	using namespace RobotPlant::Math;

	/*
	Robot motion segment type.
	*/
	enum MotionSegmentType_Kine
	{
		MotionSegment_Kine_Joint,//this segment's desired target is in joint space.
		MotionSegment_Kine_CartLinear,//this segment's desired target is in Cartesian space, and the trajectory is linear.
		MotionSegment_Kine_CartCycle//this segment's desired target is in Cartesian space, and the trajectory is linear.
	};

	/*
	This motion trajectory is composed of several segments.
	*/
	class RP_MECHANISM_API MotionSegment_Kine
	{
	public:
		bool IsZone;//true means this motion segment is zone.
		MotionSegmentType_Kine MotionSegmentType;//Motion segment type.

		ColumnVector StartJoints;//Joint values of the start point, valid if MotionSegmentType is MotionSegment_Kine_Joint.
		Frame3 StartFrame;//Cartesian frame of the start point w.r.t global frame, valid if MotionSegmentType is MotionSegment_Kine_CartLinear or MotionSegment_Kine_CartCycle.
		
		ColumnVector TargetJoints;//Joint values of the target point, valid if MotionSegmentType is MotionSegment_Kine_Joint.
		Frame3 TargetFrame;//Cartesian frame of the target point w.r.t global frame, valid if MotionSegmentType is MotionSegment_Kine_CartLinear or MotionSegment_Kine_CartCycle.
		
		Vector3 CenterPoint;//Circle center point translation w.r.t global frame, valid if MotionSegmentType is MotionSegment_Kine_CartCycle.
		
		double ChangeLength;//The trajectory length from start point to target point w.r.t global frame, valid if MotionSegmentType is MotionSegment_Kine_CartLinear or MotionSegment_Kine_CartCycle.
		Quaternion ChangeOrient;//The orientation change from start point to target point w.r.t global frame, valid if MotionSegmentType is MotionSegment_Kine_CartLinear or MotionSegment_Kine_CartCycle.

		Frame3 ToolFrame;//Tool frame which will be moved to the TargetFrame, valid if MotionSegmentType is MotionSegment_Kine_CartLinear or MotionSegment_Kine_CartCycle.
		double Speed;//Desired motion speed of tool frame along the motion trajectory.
		double FlyByZone;//target's flyby zone.
	public:
		MotionSegment_Kine();
		bool operator == ( const MotionSegment_Kine& arg );
		bool operator < ( const MotionSegment_Kine& arg );
	};

	RP_MECHANISM_TEMPLATE template class RP_MECHANISM_API std::vector<MotionSegment_Kine>;

	/*
	The MotionPlanning is used to control robot's motion according to input.
	It uses famous PD control to calculate robot tool frame acceleration in Cartesian space for current robot tool frame pose and velocity. 
	Acc = Kp*(dPose - Pose) + Kd*(dVel - Vel) + dAcc
	Pose: current robot tool frame pose.
	Vel: current robot tool frame velocity.
	Acc: calculated acceleration for current robot tool frame.
	dPose: desired robot tool frame pose.
	dVel: desired robot tool frame velocity.
	dAcc: desired current robot tool frame acceleration.	
	*/
	class RP_MECHANISM_API MotionPlanning_Kine: public MotionPlanning
	{
	public:
		MotionPlanning_Kine(Chain* pRobot, double stepInterval = 0.01);

		/*
		Destructor
		*/
		virtual ~MotionPlanning_Kine();

		/*
		@brief: gets Kd parameter.
		@return: Kd parameter.
		*/
		double Get_Kd();

		/*
		@brief: gets Kp parameter.
		@return: Kp parameter.
		*/
		double Get_Kp();

		/*
		@brief: sets Kd parameter.
		@param: Kd parameter.
		*/
		void Set_Kd(const double & Kd);

		/*
		@brief: sets Kp parameter.
		@param: Kp parameter.
		*/
		void Set_Kp(const double & Kp);

		/*
		@brief: moves robot tool frame with specified velocity.
		@param vel: desired tool frame move velocity.
		@param global: if true, vel is w.r.t global frame, else it's w.r.t tool frame.
		*/
		virtual void MoveForward(Velocity vel, bool global = false);

		/*
		@brief: moves robot to make all joint values as toJointValues.
		@param toPoint: desired target joint values in joint space.
		@param speed: desired motion speed of joints. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in Cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot all joints will reach and stop at toPoint.
		*/
		virtual void MoveJoint(const ColumnVector toJointValues, double speed, double flyByZone = 0);

		/*
		@brief: moves robot tool frame to toPoint w.r.t object frame.
		@param toPoint: desired target frame w.r.t object frame.
		@param speed: desired motion speed of joints. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot tool frame will reach and stop at toPoint.
		*/
		virtual void MoveCart(const Frame3 toPoint, double speed, double flyByZone = 0);

		/*
		@brief: moves robot tool frame to toPoint w.r.t object frame and make the path linearly.
		@param toPoint: desired target frame w.r.t object frame.
		@param speed: desired motion speed of ToolFrame. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot tool frame will reach and stop at toPoint.
		*/
		virtual void MoveCart_Linear(const Frame3 toPoint, double speed, double flyByZone = 0);

		/*
		@brief: stops robot motion.
		@param followPath: true means robot will move along current path and reduce motion speed until 0;
		false means robot will stop as soon as possible without follow current path.
		*/
		virtual void Stop(const bool followPath);

		/*
		@brief: moves robot to next step and save joint values, velocities, accelerations and torques.
		@return: returns >0 means this motion step successful, 0 means motion completed and stopped, <0 means failed.
		@remark: after invokes this method, robot's joint values, velocities, accelerations are modified.
		*/
		virtual int NextStep();

	private:
		/*
		@brief: calculates joint acceleration.
		@param: currentPos: current maximum added joint value.
		@param: currentVel: current maximum joint velocity.
		@param: segmentLength: current segment maximum joint value.
		@param: segmentSpeed: current segment maximum joint speed.
		@returns: maximum joint acceleration.
		*/
		double Cmd_Acc(const double currentPos, const double currentVel,const double & segmentLength, const double & segmentSpeed);

		/*
		@brief: inverse calculate joint values.
		@param: endEffecter: end effector's frame w.r.t robot's base frame.
		@param: jointValues: returned robot joint values.
		@returns: true means inverse calculation successful, otherwise failed.
		*/
		bool GetJointValues(Frame3 endEffecter, ColumnVector& jointValues);

	private:
		std::vector<MotionSegment_Kine> _motionSegments; //motion segments
		double _currentSegmentPos;// position in current moving trajectory segment.
		double _currentSegmentVel;// velocity in current moving trajectory segment.
		double _currentSegmentAcc;// acceleration in current moving trajectory segment.
		bool _isStop;// true means stop moving.
		double _Kp;  // Position error gain.
		double _Kd;  // Velocity error gain.
	};

}}